/**
* @file CtrlModule.h
* @author
* @brief 控制模块类的声明定义
* @version 1.0
* @date 2020-07-28
*
* @copyright Copyright (c) 2020
*
*/

#pragma once

#include <mutex>
#include "events/SystemEvt.h"
#include "data_struct/base/TimeWorkBase/Cloners/FunctionCloner.hpp"
#include "data_struct/base/TimeWorkBase/ModuleBase.h"
#include "modules/AssistModule/TimeModule.h"
#include "modules/CtrlModule/VelPubModule.h"
#include "modules/CtrlModule/MarkPosePubModel.h"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include <lib/ipa_room_planning.h>
#include "data_struct/base/DataSet.h"
#include "lib/pure_pursuit_controller.h"

namespace behavior_controller
{
class CtrlModule : public ModuleCloner<CtrlModule>
{
public:
    CtrlModule(const rclcpp::Node::SharedPtr &ros_node, std::mutex *lock_, std::shared_ptr<tf2_ros::Buffer> &tf_,
               std::shared_ptr<IpaRoomPlanning> ipa_room_plan_);

    ~CtrlModule() {};

    void UpdateTime100Hz(const evt::Timer100Hz &evt);

    void UpdateTime2Hz(const evt::Timer2Hz &evt);

    void InstallFunctors();

    void CtrlStateUpdate();

    void MonitorUpdate();

/*         void PubPlannerSelector();
    void PubControllerSelector();
    void PubSelector(); */
    void ZeroSpeed(double &linear_vel, double &angular_vel)
    {
        linear_vel = 0;
        angular_vel = 0;
    };

    rclcpp::Node::SharedPtr ros_node_;
    std::shared_ptr<tf2_ros::Buffer> tf_buffer = nullptr;
    std::shared_ptr<PurePursuitControllerD> controllerPtr = nullptr;

private:
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr planner_selector_pub = nullptr;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr controller_selector_pub = nullptr;
    std::string planner_names[3];
    std::string controller_names[3];
    std::string planner_name;
    std::string controller_name;
    bool selector_finished = false;

};

} // namespace behavior_controller